#include "Conm2ElementCalculator.h"
#include "Utility/Algebra/Vector3D.h"
#include "Utility/Algebra/Point.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Element/IElementService.h"
#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"

using SG::Algebra::Matrixd;
using SG::Algebra::Vector3D;
using SG::Algebra::Point;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using namespace SG::FEMSolver::Element;

void Conm2ElementCalculator::Initialize (SG::DataStructure::FEM::ElementTypes type)
{
    m_type = type;
}

void Conm2ElementCalculator::ComputeMass
(
const SG::DataStructure::FEM::CONM2Data& element,
const SG::DataStructure::FEM::NodeData&  node,
_OUT SG::Algebra::Matrixd&               mass
)
{
    // 单元坐标系下的质量矩阵
    mass = Matrixd (6, 6);
    mass (0, 0) = mass (1, 1) = mass (2, 2) = element.m_mass;
    mass (3, 3) = element.m_I1[0];
    mass (4, 4) = element.m_I2[1];
    mass (5, 5) = element.m_I3[2];
    mass (3, 4) = mass (4, 3) = -element.m_I2[0];
    mass (3, 5) = mass (5, 3) = -element.m_I3[0];
    mass (4, 5) = mass (5, 4) = -element.m_I3[1];

    const auto eleCoordId = element.m_cId;
    auto dispCoordId = node.m_dispCoord;
    const auto& coordProcessor = CoordinateProcessor::GetInstance ();

    // 计算 Basic 坐标系下的节点坐标
    Point point;
    if (-1 == eleCoordId || eleCoordId >0 || dispCoordId > 0)
    {
        coordProcessor.GetGlobalCoordinate (node, _OUT point, _OUT dispCoordId);
    }

    // 计算偏置矢量
    Vector3D offset{element.m_x[0], element.m_x[1], element.m_x[2]};
    if (-1 == eleCoordId)
    {
        offset.m_x -= point.m_x;
        offset.m_y -= point.m_y;
        offset.m_z -= point.m_z;
    }

    auto TEle = SG::Algebra::identity (6);

    // 偏置变换矩阵
    Matrixd TOffset (3, 3);
    TOffset (0, 1) =  offset.m_z;
    TOffset (0, 2) = -offset.m_y;
    TOffset (1, 0) = -offset.m_z;
    TOffset (1, 2) =  offset.m_x;
    TOffset (2, 0) =  offset.m_y;
    TOffset (2, 1) = -offset.m_x;

    if (eleCoordId >0 || dispCoordId > 0)
    {
        auto T = SG::Algebra::identity (3);
        if (eleCoordId > 0)
        {
            // 获取单元坐标系的变换矩阵
            coordProcessor.GetCoordTrans (eleCoordId, point, T);
        }
        if (dispCoordId > 0)
        {
            // 获取位移输出坐标系的变换矩阵
            Matrixd TG (3, 3);
            coordProcessor.GetCoordTrans (dispCoordId, point, _OUT TG);
            T = T * TG.Transpose ();
        }
        TOffset = TOffset * T;

        // 组装单元变换矩阵：单元坐标系到位移输出坐标系变换
        TEle.BlockFill (0, 0, T);
        TEle.BlockFill (3, 3, T);
    }
    // 组装单元变换矩阵：偏置变换
    TEle.BlockFill (0, 3, TOffset);

    // 单元质量矩阵变换到位移输出坐标系
    mass = TEle.Transpose () * mass * TEle;
}

void Conm2ElementCalculator::ComputeElementMatries (const SG::DBManager::DBServiceFactorySP dbServiceFactory)
{
    auto& assembly = AssemblyEigen::GetInstance ();

    CONM2Data        element;
    std::size_t      nodeIndex;
    std::vector<int> dofGlobalIndexSet (6);
    NodeData         node;
    static const DofSet dofSet{DOFDirections::DOF_Ux,   DOFDirections::DOF_Uy,   DOFDirections::DOF_Uz,
                               DOFDirections::DOF_Rotx, DOFDirections::DOF_Roty, DOFDirections::DOF_Rotz};
    Matrixd mass (6, 6);

    const auto& nodeService = dbServiceFactory->get<INodeService> ();
    const auto& eleService  = dbServiceFactory->get<IElementService> ();
    const auto  eleSize     = eleService->GetElementSize (m_type);

    for (std::size_t iEle{0}; iEle < eleSize; ++iEle)
    {
        eleService->FindByIndex (iEle, _OUT element);

        // 获取单元节点对应的索引
        assembly.FindNodeIndex (element.m_g[0], _OUT nodeIndex);

        // 获取单元自由度对应的全局自由度索引
        assembly.GetGlobalDOFId ({nodeIndex}, dofSet, _OUT dofGlobalIndexSet);

        // 获取单元节点数据
        nodeService->FindByIndex (nodeIndex, _OUT node);

        // 计算位移输出坐标系单元质量矩阵，并累加到总体质量矩阵
        ComputeMass (element, node, _OUT mass);
        assembly.AccumulateCoupledMass (dofGlobalIndexSet, mass);
    }
}